package com.openasv.act;

import java.io.OutputStream;

import ioio.lib.api.IOIO;
import ioio.lib.api.PwmOutput;
import ioio.lib.api.Uart;
import ioio.lib.api.Uart.Parity;
import ioio.lib.api.Uart.StopBits;


public class IoioController {

	protected IOIO ioio = null;
	protected int leftMotorPin 	= 6;
	protected int rightMotorPin = 7;
	protected int txPin = 6;
	protected int rxPin = 7;
	protected boolean analogMode = true;
	protected PwmOutput leftPwm, rightPwm;
	protected OutputStream uartOutput = null;
	
	public IoioController(IOIO ioio, boolean useAnalogMode) {
		this.ioio = ioio;
		if (useAnalogMode) {
			try {
				leftPwm = ioio.openPwmOutput(leftMotorPin, 100);
				leftPwm.setDutyCycle(0);
				rightPwm = ioio.openPwmOutput(rightMotorPin, 100);
				rightPwm.setDutyCycle(0);
			}
			catch (Exception e) {
				e.printStackTrace();
			}
		}
		else {
			try {
				Uart uart = ioio.openUart(rxPin, txPin, 9600, Parity.NONE, StopBits.ONE);
				uartOutput = uart.getOutputStream();
			}
			catch (Exception e) {
				e.printStackTrace();
			}
		}
	}

	public void setRightThruster(float value) {
		if (ioio != null) {
			if (analogMode) {
				try {
					rightPwm.setDutyCycle(value);
				}
				catch (Exception e) {
					e.printStackTrace();
				}
			}
			else {
				int val = (int)(128+(value-0.5)*64);
				try {
					uartOutput.write(val);
				}
				catch (Exception e) {
					e.printStackTrace();
				}
			}
		}
	}
	
	public void setLeftThruster(float value) {
		if (ioio != null) {
			if (analogMode) {
				try {
					leftPwm.setDutyCycle(value);
				}
				catch (Exception e) {
					e.printStackTrace();
				}
			}
			else {
				int val = (int)(128+(value-0.5)*64);
				try {
					uartOutput.write(val);
				}
				catch (Exception e) {
					e.printStackTrace();
				}
			}
		}
	}
	
}
